﻿using System;
using UnityEngine;

namespace NinjaToolbox.PhysicsSystem
{
    [Serializable]
    public class VirtualCapsuleShape : IPhysicsShape
    {
        public enum DirectionEnum { X, Y, Z }

        [HideInInspector]
        public Matrix4x4 matrix;
        public Vector3 offset;
        public float height = 1f;
        public float radius = 1f;
        public DirectionEnum direction = DirectionEnum.Y;
        public int virtualSegment = 3;

        public Bounds AABB
        {
            get
            {
                var lossyScale = matrix.lossyScale;
                var maxScale = Mathf.Max(Mathf.Max(Mathf.Abs(lossyScale.x), Mathf.Abs(lossyScale.y)), Mathf.Abs(lossyScale.z));

                return new Bounds(GetWorldCenter(), Mathf.Max(height, radius) * maxScale * Vector3.one);
            }
        }


        public Vector3 GetWorldCenter()
        {
            return matrix.MultiplyPoint3x4(offset);
        }

        public bool IsIntersects(IPhysicsShape other)
        {
            if (!AABB.Intersects(other.AABB)) return false;

            var sphereShape = other as SphereShape;
            if (sphereShape != null)
            {
                return PhysicsUtil.SphereVSVirtualCapsule(sphereShape.GetWorldCenter(), sphereShape.ConcertRadius, matrix, offset, height, radius, direction, virtualSegment);
            }

            var boxShape = other as BoxShape;
            if (boxShape != null)
            {
                return PhysicsUtil.BoxVSVirtualCapsule(boxShape.matrix, boxShape.offset, boxShape.size, matrix, offset, height, radius, direction, virtualSegment);
            }

            var virtualCapsuleShape = other as VirtualCapsuleShape;
            if (virtualCapsuleShape != null)
            {
                return PhysicsUtil.VirtualCapsuleVSVirtualCapsule(matrix, offset, height, radius, direction, virtualSegment
                    , virtualCapsuleShape.matrix, virtualCapsuleShape.offset, virtualCapsuleShape.height, virtualCapsuleShape.radius, virtualCapsuleShape.direction, virtualCapsuleShape.virtualSegment);
            }

            return false;
        }

        public bool IsIntersects(Vector3 lineBegin, Vector3 lineEnd)
        {
            throw new NotImplementedException();
        }

        public bool IsContainPoint(Vector3 point)
        {
            var result = false;

            var localCompareMassPoint = matrix.MultiplyPoint3x4(point);

            var lossyScale = matrix.lossyScale;
            var maxScale = Mathf.Max(Mathf.Max(Mathf.Abs(lossyScale.x), Mathf.Abs(lossyScale.y)), Mathf.Abs(lossyScale.z));
            var scaledRadius = radius * maxScale;
            var scaledHeight = height * maxScale;

            var remainHeight = Mathf.Max(scaledHeight - scaledRadius * 2f, 0) + 0.01f;
            var remainHeightHalf = remainHeight * 0.5f;

            var up = Vector3.up;
            var right = Vector3.right;
            var forward = Vector3.forward;

            switch (direction)
            {
                case DirectionEnum.X:
                    up = Vector3.right;
                    right = Vector3.forward;
                    forward = Vector3.up;
                    break;
                case DirectionEnum.Z:
                    up = Vector3.forward;
                    right = Vector3.up;
                    forward = Vector3.right;
                    break;
            }

            var pointA = offset + up * remainHeightHalf;
            var pointB = offset - up * remainHeightHalf;

            var abProjectionPoint = pointA + Vector3.Project(localCompareMassPoint - pointA, (pointB - pointA).normalized);

            var localCompareMassPointToCenterDistance = Vector3.Distance(localCompareMassPoint, offset);
            var pillarDistance = Vector3.Distance(pointA, pointB);

            if (localCompareMassPointToCenterDistance <= scaledRadius)//柱体内检测。
            {
                var abDistance = Mathf.Max(Vector3.Distance(abProjectionPoint, pointA), Vector3.Distance(abProjectionPoint, pointB));

                if (abDistance <= pillarDistance)
                {
                    result = true;
                }
            }
            else//不在柱体范围内则用上下两个球的范围做一次判断。
            {
                if (Vector3.Distance(pointA, localCompareMassPoint) <= scaledRadius)
                    result = true;

                else if (Vector3.Distance(pointB, localCompareMassPoint) <= scaledRadius)
                    result = true;
            }

            return result;
        }

        public Vector3 MassPointToEdge(Vector3 massPoint, Vector3 outsidePoint)
        {
            var localMassPoint = matrix.MultiplyPoint3x4(massPoint);
            var localOutsidePoint = matrix.MultiplyPoint3x4(outsidePoint);

            var centerPoint = localMassPoint + offset;

            var up = Vector3.up;
            var right = Vector3.right;
            var forward = Vector3.forward;

            switch (direction)
            {
                case DirectionEnum.X:
                    up = Vector3.right;
                    right = Vector3.forward;
                    forward = Vector3.up;
                    break;
                case DirectionEnum.Z:
                    up = Vector3.forward;
                    right = Vector3.up;
                    forward = Vector3.right;
                    break;
            }

            var remainHeight = Mathf.Max(height - radius * 2f, 0) + 0.01f;
            var remainHeightHalf = remainHeight * 0.5f;
            var pillarOrigin = centerPoint + up * -remainHeightHalf;
            var pillarEnd = centerPoint + up * remainHeightHalf;

            var insectPoint = Vector3.zero;
            var isHitCylinder = CylinderVSline(localOutsidePoint, (centerPoint - localOutsidePoint).normalized, pillarOrigin, pillarEnd, radius, out insectPoint);

            if (isHitCylinder)
            {
                return matrix.MultiplyPoint3x4(insectPoint);
            }
            else
            {
                var spherePoint1 = centerPoint + up * remainHeightHalf;
                var spherePoint2 = centerPoint + (-up) * remainHeightHalf;
                var finalSpherePoint = spherePoint1;

                if (Vector3.Distance(localOutsidePoint, spherePoint2) < Vector3.Distance(localOutsidePoint, spherePoint1))
                {
                    finalSpherePoint = spherePoint2;
                }

                BetweenLineAndSphere(finalSpherePoint, radius, centerPoint, localOutsidePoint, out spherePoint1, out spherePoint2);

                if (Vector3.Distance(spherePoint1, centerPoint) > Vector3.Distance(spherePoint2, centerPoint))
                    return matrix.MultiplyPoint3x4(spherePoint1);
                else
                    return matrix.MultiplyPoint3x4(spherePoint2);
            }
        }

        public Vector3 GetEdgeNormal(Vector3 massPoint, Vector3 outsidePoint)
        {
            return (outsidePoint - massPoint).normalized;
        }

        public void DrawGizmos(Color color)
        {
            var cacheColor = Gizmos.color;
            var cacheMatrix = Gizmos.matrix;

            Gizmos.color = color;

            var lossyScale = matrix.lossyScale;
            var maxScale = Mathf.Max(Mathf.Max(Mathf.Abs(lossyScale.x), Mathf.Abs(lossyScale.y)), Mathf.Abs(lossyScale.z));

            Gizmos.matrix = Matrix4x4.TRS(GetWorldCenter(), matrix.rotation, Vector3.one * maxScale);

            var remainHeight = Mathf.Max(height - radius * 2f, 0);
            var remainHeightHalf = remainHeight * 0.5f;

            var up = Vector3.up;
            var right = Vector3.right;
            var forward = Vector3.forward;

            switch (direction)
            {
                case DirectionEnum.X:
                    up = Vector3.right;
                    right = Vector3.forward;
                    forward = Vector3.up;
                    break;
                case DirectionEnum.Z:
                    up = Vector3.forward;
                    right = Vector3.up;
                    forward = Vector3.right;
                    break;
            }

            var lastPoint1 = (Vector3?)null;
            var lastPoint2 = (Vector3?)null;
            for (var i = 0f; i <= 180f; i += 9f)
            {
                var point1 = up * -remainHeightHalf + Quaternion.AngleAxis(i, right) * forward * radius;
                var point2 = up * -remainHeightHalf + Quaternion.AngleAxis(i, -forward) * right * radius;

                Gizmos.DrawLine(offset + lastPoint1.GetValueOrDefault(point1), offset + point1);
                Gizmos.DrawLine(offset + lastPoint2.GetValueOrDefault(point2), offset + point2);

                lastPoint1 = point1;
                lastPoint2 = point2;
            }

            lastPoint1 = (Vector3?)null;
            for (var i = 0f; i <= 360f; i += 9f)
            {
                var point1 = up * -remainHeightHalf + Quaternion.AngleAxis(i, up) * forward * radius;

                Gizmos.DrawLine(offset + lastPoint1.GetValueOrDefault(point1), offset + point1);

                lastPoint1 = point1;
            }

            Gizmos.DrawLine(offset + forward * radius + up * -remainHeightHalf, offset + forward * radius + up * remainHeightHalf);
            Gizmos.DrawLine(offset + -forward * radius + up * -remainHeightHalf, offset + -forward * radius + up * remainHeightHalf);

            Gizmos.DrawLine(offset + right * radius + up * -remainHeightHalf, offset + right * radius + up * remainHeightHalf);
            Gizmos.DrawLine(offset + -right * radius + up * -remainHeightHalf, offset + -right * radius + up * remainHeightHalf);

            lastPoint1 = (Vector3?)null;
            lastPoint2 = (Vector3?)null;
            for (var i = 0f; i <= 180f; i += 9f)
            {
                var point1 = up * remainHeightHalf + Quaternion.AngleAxis(i, -right) * forward * radius;
                var point2 = up * remainHeightHalf + Quaternion.AngleAxis(i, forward) * right * radius;

                Gizmos.DrawLine(offset + lastPoint1.GetValueOrDefault(point1), offset + point1);
                Gizmos.DrawLine(offset + lastPoint2.GetValueOrDefault(point2), offset + point2);

                lastPoint1 = point1;
                lastPoint2 = point2;
            }

            lastPoint1 = (Vector3?)null;
            for (var i = 0f; i <= 360f; i += 9f)
            {
                var point1 = up * remainHeightHalf + Quaternion.AngleAxis(i, up) * forward * radius;

                Gizmos.DrawLine(offset + lastPoint1.GetValueOrDefault(point1), offset + point1);

                lastPoint1 = point1;
            }

            var bottom = offset - height * 0.5f * up;
            var top = offset + height * 0.5f * up;
            var step = (top - bottom) / Mathf.Max(1, virtualSegment + 1);

            Gizmos.color = Color.Lerp(Gizmos.color, Color.clear, 0.38f);

            for (int i = 1; i <= virtualSegment; i++)
            {
                var spherePosition = bottom + step * i;

                Gizmos.DrawWireSphere(spherePosition, radius);
            }

            Gizmos.color = cacheColor;
            Gizmos.matrix = cacheMatrix;
        }

        bool BetweenLineAndSphere(
            Vector3 sphereCenter, float sphereRadius,
            Vector3 point1, Vector3 point2,
            out Vector3 intersection1, out Vector3 intersection2)
        {
            float t;

            var dx = point2.x - point1.x;
            var dy = point2.y - point1.y;
            var dz = point2.z - point1.z;

            var a = dx * dx + dy * dy + dz * dz;
            var b = 2 * (dx * (point1.x - sphereCenter.x) + dy * (point1.y - sphereCenter.y) + dz * (point1.z - sphereCenter.z));
            var c = (point1.x - sphereCenter.x) * (point1.x - sphereCenter.x)
                + (point1.y - sphereCenter.y) * (point1.y - sphereCenter.y)
                + (point1.z - sphereCenter.z) * (point1.z - sphereCenter.z) - sphereRadius * sphereRadius;

            var determinate = b * b - 4 * a * c;

            // Two solutions.
            t = (-b + Mathf.Sqrt(determinate)) / (2 * a);
            intersection1 = new Vector3(point1.x + t * dx, point1.y + t * dy, point1.z + t * dz);
            t = (-b - Mathf.Sqrt(determinate)) / (2 * a);
            intersection2 = new Vector3(point1.x + t * dx, point1.y + t * dy, point1.z + t * dz);

            if (intersection1.normalized == Vector3.zero && intersection2.normalized == Vector3.zero)
                return false;
            else
                return true;
        }

        bool CylinderVSline(Vector3 start, Vector3 dir, Vector3 aVec, Vector3 bVec, float radius, out Vector3 intersectPoint)
        {
            intersectPoint = Vector3.zero;

            // Solution : http://www.gamedev.net/community/forums/topic.asp?topic_id=467789

            var ab = bVec - aVec;
            var ao = start - aVec;
            var aoxAb = Vector3.Cross(ao, ab);
            var vxAb = Vector3.Cross(dir, ab);
            var ab2 = Vector3.Dot(ab, ab);
            var a = Vector3.Dot(vxAb, vxAb);
            var b = 2 * Vector3.Dot(vxAb, aoxAb);
            var c = Vector3.Dot(aoxAb, aoxAb) - (radius * radius * ab2);
            var d = b * b - 4 * a * c;
            if (d < 0) return false;
            float time = (-b - Mathf.Sqrt(d)) / (2 * a);
            if (time < 0) return false;

            intersectPoint = start + dir * time;

            var projectionPoint = aVec + Vector3.Project(intersectPoint - aVec, ab.normalized);
            var distance1 = Mathf.Max(Vector3.Distance(projectionPoint, aVec), Vector3.Distance(projectionPoint, bVec));
            var distance2 = Vector3.Distance(aVec, bVec);

            if (distance1 >= distance2)
                return false;

            return true;
        }

        Matrix4x4 IPhysicsShape.Matrix { get { return matrix; } set { matrix = value; } }
    }
}
